// Copyright 2021 ROBOTIS CO., LTD.
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

/*******************************************************************************
 * This example is written for DYNAMIXEL X(excluding XL-320) and MX(2.0) series with U2D2.
 * For other series, please refer to the product eManual and modify the Control Table addresses and other definitions.
 * To test this example, please follow the commands below.
 *
 * Open terminal #1
 * $ roscore
 *
 * Open terminal #2
 * $ rosrun dynamixel_sdk_examples bulk_read_write_node
 *
 * Open terminal #3 (run one of below commands at a time)
 * $ rostopic pub -1 /bulk_set_item dynamixel_sdk_examples/BulkSetItem "{id1: 1, id2: 2, item1: 'position', item2: 'LED', value1: 1000, value2: 1}"
 * $ rostopic pub -1 /bulk_set_item dynamixel_sdk_examples/BulkSetItem "{id1: 1, id2: 2, item1: 'LED', item2: 'position', value1: 1, value2: 1000}"
 * $ rosservice call /bulk_get_item "{id1: 1, id2: 2, item1: 'position', item2: 'LED'}"
 *
 * Author: Jaehyun Shim
*******************************************************************************/

#include <ros/ros.h>
#include <fstream>
#include <ctime>
#include "std_msgs/String.h"
#include "dynamixel_sdk_examples/BulkGetItem_Port4.h"
#include "dynamixel_sdk_examples/BulkGetItem_Port3.h"
#include "dynamixel_sdk_examples/BulkSetItem_Port4.h"
#include "dynamixel_sdk_examples/BulkSetItem_Port3.h"
#include "dynamixel_sdk/dynamixel_sdk.h"

using namespace dynamixel;

int main(int argc, char ** argv)
{
  ros::init(argc, argv, "bulk_read_write_node_restore");
  ros::NodeHandle nh;
  
  ros::Publisher self_Pub_Port4=nh.advertise<dynamixel_sdk_examples::BulkSetItem_Port4>("/bulk_set_item_port4", 1);
  ros::Publisher self_Pub_Port3=nh.advertise<dynamixel_sdk_examples::BulkSetItem_Port3>("/bulk_set_item_port3", 1);
  ros::ServiceClient self_bulk_Client_Port4=nh.serviceClient<dynamixel_sdk_examples::BulkGetItem_Port4>("/bulk_get_item_port4");
  ros::ServiceClient self_bulk_Client_Port3=nh.serviceClient<dynamixel_sdk_examples::BulkGetItem_Port3>("/bulk_get_item_port3");

  dynamixel_sdk_examples::BulkSetItem_Port4 bulk_msg_port4_self;
  dynamixel_sdk_examples::BulkSetItem_Port3 bulk_msg_port3_self;
  dynamixel_sdk_examples::BulkGetItem_Port4 bulk_Port4_self;
  dynamixel_sdk_examples::BulkGetItem_Port3 bulk_Port3_self;

  ros::Rate rate(50);

  bulk_Port4_self.request.id1 = 1;     //舵机ID修改
  bulk_Port4_self.request.id2 = 2;
  bulk_Port3_self.request.id1 = 3;
  bulk_Port3_self.request.id2 = 4;
  bulk_Port3_self.request.id3 = 5;
  bulk_Port4_self.request.item1 = "position";
  bulk_Port4_self.request.item2 = "position";
  bulk_Port3_self.request.item1 = "position";
  bulk_Port3_self.request.item2 = "position";
  bulk_Port3_self.request.item3 = "position";

  bulk_msg_port4_self.id1=1;       //舵机ID修改
  bulk_msg_port4_self.id2=2;    
  bulk_msg_port3_self.id1=3;      
  bulk_msg_port3_self.id2=4;     
  bulk_msg_port3_self.id3=5;       
  bulk_msg_port4_self.item1="position";
  bulk_msg_port4_self.item2="position";
  bulk_msg_port3_self.item1="position";
  bulk_msg_port3_self.item2="position";
  bulk_msg_port3_self.item3="position";

  for(int j=0; j<500; j++){
    rate.sleep();
  }
  self_bulk_Client_Port4.call(bulk_Port4_self);
  self_bulk_Client_Port3.call(bulk_Port3_self);

  std::ifstream inFlie;
  int life_co;

  inFlie.open("/home/nuc/qt_file2/intco.txt");
  if(!inFlie){
    std::cerr<<"Unable to open file intco.txt";
  }else{
    inFlie >> life_co;
    std::cout<<"记录的升降舵机0位: "<< life_co <<std::endl;
  }

  for(double i=0; i<500; ++i){
    bulk_msg_port4_self.value1=int(double(bulk_Port4_self.response.value1) * (1-i/500) + life_co * (i/500));     //舵机修改为吉大板子上的
    bulk_msg_port4_self.value2=int(double(bulk_Port4_self.response.value2) * (1-i/500) + 2048 * (i/500));
    bulk_msg_port3_self.value1=int(double(bulk_Port3_self.response.value1) * (1-i/500) + 2048 * (i/500));
    bulk_msg_port3_self.value2=int(double(bulk_Port3_self.response.value2) * (1-i/500) + 2048 * (i/500));
    bulk_msg_port3_self.value3=int(double(bulk_Port3_self.response.value3) * (1-i/500) + (2048 + 341) * (i/500));
    self_Pub_Port4.publish(bulk_msg_port4_self);
    self_Pub_Port3.publish(bulk_msg_port3_self);
    rate.sleep();
  }  

  return 0;
}
